#include "sys.h"
#include "FreeRTOS.h"
#include "task.h"
#include "delay.h"
#include "adc_battery.h"
#include "IMU.h"
#include "uart.h"
#include "vl53lxx.h"
#include "optical_flow.h"
#include "pwm.h"
#include "general_gpio.h"
#include "timer.h"
#include "Tasks.h"

#define START_TASK_PRIO			1
#define START_STK_SIZE 			256  
TaskHandle_t StartTask_Handler;

#define IMU_TASK_PRIO			4
#define IMU_STK_SIZE 			1024  
TaskHandle_t IMUTask_Handler;

#define AttitudeInner_TASK_PRIO			5
#define AttitudeInner_STK_SIZE 			1024  
TaskHandle_t AttitudeInnerTask_Handler;

#define PositionInner_TASK_PRIO			6
#define PositionInner_STK_SIZE 			1024  
TaskHandle_t PositionInnerTask_Handler;

#define Laser_TASK_PRIO			7
#define Laser_STK_SIZE 			1024  
TaskHandle_t LaserTask_Handler;

#define OptionFlow_TASK_PRIO			8
#define OptionFlow_STK_SIZE 			1024  
TaskHandle_t OptionFlowTask_Handler;

#define AttitudeOuter_TASK_PRIO			9
#define AttitudeOuter_STK_SIZE 			1024  
TaskHandle_t AttitudeOuterTask_Handler;

#define Data_Deal_TASK_PRIO			10
#define Data_Deal_STK_SIZE 			1024  
TaskHandle_t Data_DealTask_Handler;


#define Data_Report_TASK_PRIO			11
#define Data_Report_STK_SIZE 			1024  
TaskHandle_t Data_ReportTask_Handler;

#define Battery_TASK_PRIO			12
#define Battery_STK_SIZE 			1024  
TaskHandle_t BatteryTask_Handler;

#define SYS_CLK_MHZ 				168
#define UART_BOUNDRATE				115200

void start_task(void *pvParameters);

int main(void)
{ 
	delay_init(SYS_CLK_MHZ);
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
	
	Uart3_to_BT_Init(UART_BOUNDRATE);
	Uart1_to_PC_Init(UART_BOUNDRATE);
	OpticalFlow_Init();
	IICtoLaser_Init();
	PWM_Init();
	ADC_Battery_Init();
	General_Gpioinit();
	Tim3_Init();
	Tim4_Init();
	Tim5_Clock_Init();
	IMU_HardwareInit();
	
    xTaskCreate((TaskFunction_t )start_task,            
                (const char*    )"start_task",          
                (uint16_t       )START_STK_SIZE,        
                (void*          )NULL,                  
                (UBaseType_t    )START_TASK_PRIO,       
                (TaskHandle_t*  )&StartTask_Handler);     
	//开启FreeRTOS
    vTaskStartScheduler();          
}


void start_task(void *pvParameters)
{
    taskENTER_CRITICAL();  
	
    xTaskCreate((TaskFunction_t )IMU_task,            
                (const char*    )"IMU_task",          
                (uint16_t       )START_STK_SIZE,        
                (void*          )NULL,                  
                (UBaseType_t    )START_TASK_PRIO,       
                (TaskHandle_t*  )&StartTask_Handler);	
				
    xTaskCreate((TaskFunction_t )Laser_task,            
                (const char*    )"Laser_task",          
                (uint16_t       )Laser_STK_SIZE,        
                (void*          )NULL,                  
                (UBaseType_t    )Laser_TASK_PRIO,       
                (TaskHandle_t*  )&LaserTask_Handler);
				
    xTaskCreate((TaskFunction_t )OptionFlow_task,            
                (const char*    )"OptionFlow_task",          
                (uint16_t       )OptionFlow_STK_SIZE,        
                (void*          )NULL,                  
                (UBaseType_t    )OptionFlow_TASK_PRIO,       
                (TaskHandle_t*  )&OptionFlowTask_Handler);
				
    xTaskCreate((TaskFunction_t )Battery_task,            
                (const char*    )"Battery_task",          
                (uint16_t       )Battery_STK_SIZE,        
                (void*          )NULL,                  
                (UBaseType_t    )Battery_TASK_PRIO,       
                (TaskHandle_t*  )&BatteryTask_Handler);
				
    xTaskCreate((TaskFunction_t )Data_Deal_task,            
                (const char*    )"Data_Deal_task",          
                (uint16_t       )Data_Deal_STK_SIZE,        
                (void*          )NULL,                  
                (UBaseType_t    )Data_Deal_TASK_PRIO,       
                (TaskHandle_t*  )&Data_DealTask_Handler);
				
    xTaskCreate((TaskFunction_t )Data_Report_task,            
                (const char*    )"Data_Report_task",          
                (uint16_t       )Data_Report_STK_SIZE,        
                (void*          )NULL,                  
                (UBaseType_t    )Data_Report_TASK_PRIO,       
                (TaskHandle_t*  )&Data_ReportTask_Handler);
				
    xTaskCreate((TaskFunction_t )AttitudeInner_Control_task,            
                (const char*    )"AttitudeInner_Control_task",          
                (uint16_t       )AttitudeInner_STK_SIZE,        
                (void*          )NULL,                  
                (UBaseType_t    )AttitudeInner_TASK_PRIO,       
                (TaskHandle_t*  )&AttitudeInnerTask_Handler);
				
    xTaskCreate((TaskFunction_t )AttitudeOuter_Control_task,            
                (const char*    )"AttitudeOuter_Control_task",          
                (uint16_t       )AttitudeOuter_STK_SIZE,        
                (void*          )NULL,                  
                (UBaseType_t    )AttitudeOuter_TASK_PRIO,       
                (TaskHandle_t*  )&AttitudeOuterTask_Handler);	
				
    xTaskCreate((TaskFunction_t )PositionInner_Control_task,            
                (const char*    )"PositionInner_Control_task",          
                (uint16_t       )PositionInner_STK_SIZE,        
                (void*          )NULL,                  
                (UBaseType_t    )PositionInner_TASK_PRIO,       
                (TaskHandle_t*  )&PositionInnerTask_Handler);	
	
    taskEXIT_CRITICAL(); 
	vTaskSuspend(StartTask_Handler);	
}





